import rclpy
from rclpy.node import Node
from gobang_interfaces.msg import Serial
import serial
import struct

# 串口配置（根据实际设备修改）
SERIAL_PORT = '/dev/ttyS3'  # 串口号
BAUD_RATE = 9600      # 波特率

# 协议常量
START_BYTES = b'\xFE\x01'  # 起始位
END_BYTES = b'\xEF\x10'    # 结束位

def calculate_checksum(command:int, data:list)->int:
    """计算校验和：指令位 + 所有数据字节之和,保留低2字节"""
    total = command
    for byte in data:
        total += byte
    return total & 0xFFFF  # 取低16位

class SerialNode(Node):

    def __init__(self):
        super().__init__('serial_node_sub')
        self.ser = serial.Serial(SERIAL_PORT, BAUD_RATE)
        self.get_logger().info(f"监听串口 {SERIAL_PORT}，波特率 {BAUD_RATE}...")
        self.serial_sub = self.create_subscription(Serial,'serial_sub', self.serial_callback, 10)

    def serial_callback(self, serial_msg: Serial):
        """处理订阅的消息（如果需要）"""
        self.get_logger().info(f"收到串口数据, 指令:{serial_msg.command}, 数据:{serial_msg.data}")
        # 发送到串口
        if self.ser.is_open:
            try:
                # 组装数据包
                packet = (
                    START_BYTES +
                    struct.pack('B', serial_msg.command) +  # 将uint8转换为bytes
                    bytes(serial_msg.data) +               # 将uint8[]转换为bytes
                    struct.pack('>H', calculate_checksum(serial_msg.command, serial_msg.data)) +
                    END_BYTES
                )
                self.ser.write(packet)
                self.get_logger().info(f"发送数据包：{packet.hex(' ')}")
            except Exception as e:
                self.get_logger().error(f"发送错误：{str(e)}")
        
def main(args=None):
    rclpy.init(args=args)
    serial_node = SerialNode()
    rclpy.spin(serial_node)
    serial_node.destroy_node()
    rclpy.shutdown()